#include <iostream>
#include <string>
#include <pcl/io/ply_io.h>  
#include <pcl/io/pcd_io.h>
#include <pcl/io/vtk_lib_io.h> 
#include <pcl/registration/ndt.h> 
#include <pcl/point_types.h> 
#include <pcl/registration/icp.h> 
#include <pcl/visualization/pcl_visualizer.h> 
#include <pcl/console/time.h>   
#include <pcl/filters/voxel_grid.h> 
#include <pcl/common/transforms.h> 
#include <pcl/io/vtk_lib_io.h> 
#include <vector>
#include <pcl/point_cloud.h>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <pcl/keypoints/sift_keypoint.h>
#include <math.h>

using namespace std;


typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud<PointT> PointCloudT;



bool get_point(const string& filename, vector<pcl::PointXYZ>& points_vector)
{
	ifstream fs;
	fs.open(filename.c_str(), ios::binary);
	if (!fs.is_open() || fs.fail())
	{
		fs.close();
		return (false);
	}

	string line;
	vector<string> st;

	while (!fs.eof())
	{
		getline(fs, line);
		// Ignore empty lines
		if (line.empty())
			continue;

		// Tokenize the line
		boost::trim(line);
		boost::split(st, line, boost::is_any_of("\t\r "), boost::token_compress_on);

		if (st.size() != 3)
			continue;

		pcl::PointXYZ point;
		point.x = double(atof(st[0].c_str()));
		point.y = double(atof(st[1].c_str()));
		point.z = double(atof(st[2].c_str()));
		points_vector.push_back(point);
	}
	fs.close();

	return (true);
}

int main(int argc, char* argv[]) {
	//  目标点云的点，初始点云的点。 txt,txt --> martrix
	if (argc != 3) {
		printf("{\"success\":false}");
		return (0);
	}

	std::vector<std::vector<PointT>> Points_from(2, vector<PointT>());
	string filename1 = argv[1];
	string filename2 = argv[2];
	if (!(get_point(filename1, Points_from[0]) && get_point(filename2, Points_from[1]))) {
		cout << 1 << endl;
		printf("{\"success\":false}");
		return (0);
	}
	if (Points_from[0].size() == 0 || Points_from[0].size() != Points_from[1].size()) {
		cout << 2 << endl;
		printf("{\"success\":false}");
		return (0);
	}

	int pairs_num = Points_from[0].size();
	
	Eigen::Vector3f a_pt{ 0,0,0 };
	Eigen::Vector3f a_ps{ 0,0,0 };
	Eigen::Matrix3f H;
	H.setZero(3, 3);

	for (int i = 0; i < pairs_num; ++i) {
		a_pt += Points_from[0][i].getVector3fMap();
		a_ps += Points_from[1][i].getVector3fMap();
		
	}
	a_pt /= 4.0;
	a_ps /= 4.0;
	
	for (int i = 0; i < pairs_num; ++i) {
		H += (Points_from[1][i].getVector3fMap()-a_ps) * (Points_from[0][i].getVector3fMap()-a_pt).transpose();
	}
	
	Eigen::JacobiSVD<Eigen::MatrixXf> svd_holder(H,
		Eigen::ComputeThinU |
		Eigen::ComputeThinV);
	// 构建SVD分解结果
	Eigen::MatrixXf U = svd_holder.matrixU();
	Eigen::MatrixXf V = svd_holder.matrixV();


	Eigen::Matrix3f R = V * (U.transpose());
	auto t = a_pt - R * a_ps;

	printf("{\"success\":true");

	printf(",\"matrix\":[");
	printf("%6.3f, %6.3f ,%6.3f, %6.3f ,", R(0, 0), R(0, 1), R(0, 2), t(0));
	printf("%6.3f, %6.3f, %6.3f ,%6.3f ,", R(1, 0), R(1, 1), R(1, 2), t(1));
	printf("%6.3f, %6.3f, %6.3f, %6.3f, ", R(2, 0), R(2, 1), R(2, 2), t(2));
	printf("%6.3f, %6.3f, %6.3f ,%6.3f", (double)0, (double)0, (double)0, (double)1);
	printf("]");
	printf("}");
	printf("\n");
	return (0);
}